/**
 * Copyright (c) 2024, https://www.zhz.com/
 * All rights reserved.
 * $Revision: 1.0 $
 * $Date: 2024/7 $
 * $Author: MSun $
 ******************************************************************************/

#include "apriltag_ros/common_functions.h"
#include <Eigen/Dense>
#include <vector>
#include <iostream>

namespace apriltag_ros
{
// 计算Savitzky-Golay滤波器中心点的系数
Eigen::VectorXd TagDetector::computeSGCoeffs(int windowSize, int polyOrder) {
    int halfWindow = (windowSize - 1) / 2;
    Eigen::MatrixXd H(windowSize, polyOrder + 1);

    for (int i = -halfWindow; i <= halfWindow; ++i) {
        for (int j = 0; j <= polyOrder; ++j) {
            H(i + halfWindow, j) = std::pow(i, j);
        }
    }

    Eigen::MatrixXd HTH = H.transpose() * H;
    Eigen::MatrixXd HTH_inv = HTH.inverse();
    Eigen::MatrixXd coeffs = H * HTH_inv * H.transpose();

    return coeffs.row(windowIndex_);  // 返回中心点的系数
}

// 使用Savitzky-Golay滤波器平滑最新数据点的函数
Eigen::VectorXd TagDetector::sgolayfilt(const std::deque<Eigen::VectorXd>& data) {
    int halfWindow = (windowSize_ - 1) / 2;
    int num = data.size();
    int dim = data[0].size();
    Eigen::VectorXd& coeffs = SGCoeffs_;

    Eigen::VectorXd smoothedData(dim);

    for (int j = 0; j < dim; ++j) {
        Eigen::VectorXd segment(windowSize_);
        for (int k = -halfWindow; k <= halfWindow; ++k) {
            int index = std::min(std::max(num - 1 + k, 0), num - 1);
            segment(k + halfWindow) = data[index][j];
        }
        smoothedData(j) = coeffs.dot(segment);
    }

    return smoothedData;
}

}  //  namespace apriltag_ros
